/*
 * app_net_recv_thread.c
 *
 *  Created on: 2024年6月27日
 *      Author: xhq
 */
/* include */
#include <time.h>
#include <sys/time.h>
#include <sys/stat.h> 
#include "app_thread_modules.h"
#include "net_protocol.h"
#include "util_factory.h"
#include "bsp_mipi.h"

/* define */
#define SEND_DATA_LEN      32768
/* typedef */

/* function */
/*****************************************************************************/
/**
 * @brief 网络数据编码,并且加上帧头帧尾
 * @param input 输入数据指针
 * @param input_len 输入数据长度
 * @param output 输出缓冲区指针
 * @param max_output_len 输出缓冲区最大容量
 * @param output_len 实际编码后的数据长度
 * @return 0--成功，1--参数错误，2--缓冲区不足
 *
******************************************************************************/
uint32_t encode(const uint8_t *input, uint32_t input_len, uint8_t *output, uint32_t max_output_len, uint32_t *output_len) 
{
    // 参数检查
    if ((!input) || (input_len == 0) || (!output) || (max_output_len == 0) || (!output_len)) 
    {
        printf("encode input param error.\n");
        return 1;
    }

    size_t out_idx = 0;
    // printf("input_len = %u.\n", input_len);

    // 添加帧头
    output[out_idx++] = NET_FRAME_HEAH;

    for (size_t i = 0; i < input_len; i++) 
    {
        uint8_t c = input[i];
 
        // 检查剩余空间是否足够
        if ((c == 0xC0 || c == 0xDB) ? (out_idx + 2 > max_output_len): (out_idx + 1 > max_output_len)) 
        {
            printf("input[i] = 0x%02x, out_idx = %d, max_output_len = %u.\n", input[i], out_idx, max_output_len);
            return 2;  // 缓冲区不足
        }

#if 1   // 0--不进行数据编码；1--进行数据编码
        // 执行转义编码
        if (c == 0xC0) 
        {
            output[out_idx++] = 0xDB;
            output[out_idx++] = 0xDC;
        } 
        else if (c == 0xDB) 
        {
            output[out_idx++] = 0xDB;
            output[out_idx++] = 0xDD;
        } 
        else 
        {
            output[out_idx++] = c;
        }
#else   // debug使用
        // 不执行转义编码
        output[out_idx++] = c;
#endif
    }

    // 添加帧尾
    output[out_idx++] = NET_FRAME_END;

    *output_len = out_idx;

    return 0;
}


/*****************************************************************************/
/**
 * @brief 网络数据解码,并且去掉帧头帧尾
 * @param input 输入数据指针
 * @param input_len 输入数据长度
 * @param output 输出缓冲区指针
 * @param max_output_len 输出缓冲区最大容量
 * @param output_len 实际解码后的数据长度
 * @return 0成功，-1参数错误，-2帧头或帧尾错误, -3缓冲区不足，-4非法转义序列
*
******************************************************************************/
int decode(const uint8_t *input, uint32_t input_len, uint8_t *output, uint32_t max_output_len, uint32_t *output_len) 
{
    // 参数检查
    if (!input || !output || !output_len) 
    {
        return -1;
    }

#if 0   // debug
    printf("decode %d Byte data: ", input_len);
    for(uint32_t i = 0; i < input_len; i++)
    {
        printf("0x%02x ", input[i]);
    }
    printf("\n");
#endif

    // 检查帧头帧尾
    if ((input[0] != NET_FRAME_HEAH) || (input[input_len -1] != NET_FRAME_END))
    {
        printf("decode frame head or end err, frame head is 0x%02x, frame end is 0x%02x.\n", input[0], input[input_len - 1]);
        return -2;
    }

    uint8_t l_u8recv_data[UDP_RECV_BUFF_SIZE] = {0};
    uint32_t l_u32data_len = 0;

    // 去掉帧头帧尾
    l_u32data_len = input_len - 2;
    memcpy(l_u8recv_data, (void *)(&input[1]), l_u32data_len);

    size_t out_idx = 0;
    size_t i = 0;

    // for (size_t i = 0; i < l_u32data_len; i++) 
    while(i < l_u32data_len)
    {
        // 检查输出缓冲区剩余空间
        if (out_idx + 1 > max_output_len) 
        {
            return -3;
        }
        
        if (l_u8recv_data[i] == 0xDB)   // DB后面必须是DC或者DD
        {
            // 处理转义序列
            if (i + 1 >= l_u32data_len) 
            {
                return -4;  // 非法结尾
            }

            if (l_u8recv_data[i+1] == 0xDC) 
            {
                output[out_idx++] = 0xC0;
            } 
            else if (l_u8recv_data[i+1] == 0xDD) 
            {
                output[out_idx++] = 0xDB;
            } 
            else 
            {
                return -4;  // 非法转义
            }

            i += 2;  // 跳过两个字节
        } 
        else 
        {
            // 普通字节直接复制
            output[out_idx++] = l_u8recv_data[i];
            i += 1;
        }
    }

    *output_len = out_idx;

#if 0
    printf("output %d Byte data: ", *output_len);
    for(uint32_t i = 0; i < *output_len; i++)
    {
        printf("0x%02x ", output[i]);
    }
    printf("\n");
#endif

    return 0;
}


/*****************************************************************************/
/**
 * @brief 网络数据帧，填充设备自己的身份标识信息
 * @param p_self_id_info 设备自身的标识信息地址
 * @return 0--成功，-1--失败
*
******************************************************************************/
uint32_t fill_dev_self_ID(IDENTITY *p_self_id_info)
{
    /* 参数检查 */
    if (p_self_id_info == NULL)
    {
        printf("fill_dev_self_ID para error.\n");
        return -1;
    }

    p_self_id_info->AreaCode[0] = 0x00;
    p_self_id_info->AreaCode[1] = 0x00;
    p_self_id_info->AreaCode[2] = 0x00;

    p_self_id_info->type = MMW_LIDAR;

    p_self_id_info->num = DEV_SN_NUM;

    return 0;
}

/*****************************************************************************/
/**
 * @brief 网络数据帧，帧头表内容填充，不包含0xC0帧头
 * @param recv_id_info 接收方标识信息 
 * @param oper_type 操作类型
 * @param oper_tar  操作对象
 * @param out_buf   输出缓冲区地质
 * @param output_len 输出缓冲区长度
 * @return 0--成功，-1--失败
*
******************************************************************************/
uint32_t fill_net_frame_head_table(IDENTITY recv_id_info, uint32_t oper_type, uint32_t oper_tar, uint8_t *out_buf, uint32_t output_len)
{
    uint8_t l_u8ret = 0;
    NET_FRAME_HEAD_TAB *p_frame_head = NULL;

    /* 参数检查 */
    if ((out_buf == NULL) || (output_len < sizeof(NET_FRAME_HEAD_TAB)))
    {
        printf("fill_net_frame_head_table para error.\n");
        return 1;
    }


    p_frame_head = (NET_FRAME_HEAD_TAB *)out_buf;

    p_frame_head->link_addr = NET_LINK_ADDR;
    l_u8ret = fill_dev_self_ID(&p_frame_head->id_info_1);
    if (l_u8ret)
    {
        printf("fill_dev_self_ID failed.\n");
        return 1;
    }
    
    memcpy(&p_frame_head->id_info_2, &recv_id_info, sizeof(IDENTITY));
    p_frame_head->protocol_ver = PROTOCOL_VERSION;
    p_frame_head->oper_type = oper_type;
    p_frame_head->oper_target = oper_tar;

    return 0;

}


/*****************************************************************************/
/**
*
* 网络广播6060端口数据接收任务 net recv thread
*
* @param	None.
*
* @return	None
*
* @note		None.
*
******************************************************************************/
void *app_net_broadcast_recv_thread()
{
    char l_c_thread_name[25] = "app_net_broadcast_recv";
    uint8_t *l_u8_recv_buf = NULL;
    int32_t l_int32_err = 0;
    uint32_t l_u32_recv_data_size = UDP_RECV_BUFF_SIZE;
    struct sockaddr_in l_dest_sockaddr = {0};
    socklen_t l_recv_len = sizeof(struct sockaddr_in);
    uint8_t l_u8_dec_buf[UDP_RECV_BUFF_SIZE * 2] = {0};
    uint32_t l_u32dec_data_len = 0;

    prctl(PR_SET_NAME, l_c_thread_name);

    /* 分配接收数组内存 */
    l_u8_recv_buf = malloc(l_u32_recv_data_size);
    if (l_u8_recv_buf == NULL)
    {
        printf("malloc recv buf failed!\n");
        return NULL;
    }

    printf("net broadcast start!\n");

    while (1)
    {
        /* 接收网络数据并解析 */ 
        l_int32_err = recvfrom(g_socket_info[SOCKET_BROADCAST_TYPE].fd, (char *)l_u8_recv_buf, 
                            l_u32_recv_data_size, 0, (struct sockaddr *)&l_dest_sockaddr, &l_recv_len); 
        if (l_int32_err < 0)
        {
            printf("net broadcast failed:%d\n", l_int32_err);
        }
        else
        {   
            /* 排除命令和数据端口和ip */

            // 解码及去掉枕头帧尾
            l_u32_recv_data_size = l_int32_err;
            l_int32_err = decode(l_u8_recv_buf, l_u32_recv_data_size, l_u8_dec_buf, sizeof(l_u8_dec_buf), &l_u32dec_data_len);
            if (l_int32_err)
            {
                printf("app_net_recv_thread decode failed, l_int32_err = %d\n", l_int32_err);
                continue;
            }

            /* 协议解析 */
            net_anlysis_procotol((struct sockaddr *)&l_dest_sockaddr, l_u8_recv_buf, l_u32dec_data_len);
        }
    }   
}
/*****************************************************************************/
/**
*
* 网络配置端口数据接收任务 net recv thread
*
* @param    None.
*
* @return   None
*
* @note     None.
*
******************************************************************************/
void *app_net_recv_thread()
{
    char l_c_thread_name[17] = "app_net_recv";
    uint8_t *l_u8_recv_buf = NULL;
    int32_t l_int32_err = 0;
    uint32_t l_u32_recv_buf_len = UDP_RECV_BUFF_SIZE;
    uint32_t l_u32_recv_data_size = 0;
    struct sockaddr_in l_dest_sockaddr = {0};
    socklen_t l_recv_len = sizeof(struct sockaddr_in);
    uint8_t l_u8_dec_buf[UDP_RECV_BUFF_SIZE] = {0};
    uint32_t l_u32dec_data_len = 0;
    uint32_t i = 0;

    /* 分配接收数组内存 */
    l_u8_recv_buf = malloc(l_u32_recv_buf_len);
    if (l_u8_recv_buf == NULL)
    {
        printf("malloc recv buf failed!\n");
        return NULL;
    }
    memset(l_u8_recv_buf, 0x00, l_u32_recv_buf_len);

    prctl(PR_SET_NAME, l_c_thread_name);

    while (1)
    {
        /* 接收网络数据并解析 */
        l_int32_err = recvfrom(g_socket_info[SOCKET_CMD_TYPE].fd, (char *)l_u8_recv_buf, 
                            l_u32_recv_buf_len, 0, (struct sockaddr *)&l_dest_sockaddr, &l_recv_len); 
        if (l_int32_err < 0)
        {
            printf("net recv failed:%d\n", l_int32_err);
            thread_mSleep(1000);   // 防止接收异常占用CPU
            continue;
        }

        /* 排除广播端口和ip */

        l_u32_recv_data_size = l_int32_err;
#if 0   // debug
        printf("udp recv %d Byte data: ", l_u32_recv_data_size);
        for(i = 0; i < l_u32_recv_data_size; i++)
        {
            printf("0x%02x ", l_u8_recv_buf[i]);
        }
        printf("\n");
#endif
        /* 解码及去掉枕头帧尾 */
        l_int32_err = decode(l_u8_recv_buf, l_u32_recv_data_size, l_u8_dec_buf, sizeof(l_u8_dec_buf), &l_u32dec_data_len);
        if (l_int32_err)
        {
            printf("app_net_recv_thread decode failed, l_int32_err = %d\n", l_int32_err);
            continue;
        }

        /* 协议解析 */
        net_anlysis_procotol((struct sockaddr *)&l_dest_sockaddr, l_u8_dec_buf, l_u32dec_data_len);
        memset(l_u8_recv_buf, 0x00, l_u32_recv_buf_len);
    }

    if (l_u8_recv_buf)
    {
        free(l_u8_recv_buf);
    }
}


void clear_fifo_flag_end_send(int index)  
{

    int i=0;

    g_mipi_req_info.req_mipi_num = 0;

    printf("err end collect and send!\n");
    //初始化堆积的信号量
    sem_init(&g_mipi_send_up_semaphore, 0, 0);

    //不再生产数据
    g_mipi_recv_info.mipi_index[index].need_handle_fram_num = g_mipi_req_info.req_mipi_num;
    g_mipi_recv_info.mipi_index[index^1].need_handle_fram_num = g_mipi_req_info.req_mipi_num;

    thread_uSleep(10000);

    while(g_mipi_to_up_queue[index].fifoout  !=  g_mipi_to_up_queue[index].fifoin)
    {
        //不再消费数据
        g_mipi_to_up_queue[index].m_u8valid[g_mipi_to_up_queue[index].fifoout] == INVALID_VALUE;
        g_mipi_to_up_queue[index^1].m_u8valid[g_mipi_to_up_queue[index^1].fifoout] == INVALID_VALUE;
        g_mipi_to_up_queue[index].fifoout = (g_mipi_to_up_queue[index].fifoout + 1)%QUEUE_RECV_DATA_NUM;
        g_mipi_to_up_queue[index^1].fifoout = (g_mipi_to_up_queue[index^1].fifoout + 1)%QUEUE_RECV_DATA_NUM;
    }

}



/*****************************************************************************/
/**
*
* 网络发送mipi数据任务 net send mipi data thread
*
* @param    None.
*
* @return   None
*
* @note     None.
*
******************************************************************************/
void *app_net_send_mipi_thread()
{
    char l_c_thread_name[20] = "app_net_send_mipi";
	uint32_t  l_u32Length_1 = 0, l_u32Length_2 = 0;
	uint8_t   l_u8fifoin;
    uint32_t i = 0, j = 0, m = 0, n = 0;
    ORIGNAL_MIPI_REQ mipi_data_req;
    UPLOAD_MIPI_DATA up_data;
    uint8_t l_u8temp_buf[33000] = {0};
    uint8_t l_u8snd_buf[65536] = {0};
    uint32_t l_u32temp_buf_len = 0;
    uint32_t l_u32snd_buf_len = 0;
    uint8_t l_u8ret = 0;
    uint8_t l_u8IPC_data_flag = 0;
    uint32_t l_u32_total_frame_num;
    uint32_t l_u32_frame_count;
    uint16_t l_u16_total_package_num;
    uint16_t l_u16_package_count;
    uint32_t l_u32buf_idx = 0;
    uint32_t l_u32temp_buf_idx = 0;
    uint32_t *p_frame_cnt = NULL;
    uint16_t *p_package_cnt = NULL;
    struct sockaddr * p_sockaddr = NULL;
    uint16_t l_u16_crc = 0;
    int     l_s32fd = -1;
    uint32_t l_u32front_frame_mipi = 0, l_u32after_frame_mipi = 0;
    uint32_t l_u32mipi_data_invald_cnt = 0;    // 数据处理过程中记录队列中无效数据的次数，次数超过队列数，则说明队列中已经没有有效数据了
    struct timespec ts;
    long timeout = 1000;    // 超时时间为1s
    int sem_ret = 0;
    uint32_t l_u32reupload_cnt = 0;
    
    prctl(PR_SET_NAME, l_c_thread_name);

    while (1)
    {
        
        // 确定队列里面有数，就不用检测，直接跳到数据处理的步骤
        if (!l_u8IPC_data_flag)
        {
            /* 等待信号量 */
            sem_wait(&g_mipi_send_up_semaphore);

            // 先看是否有新的半帧的mipi数据被保存
            for (i = 0; i < MIPI_CHANNEL_MAX; i++)
            {
                pthread_mutex_lock(&g_Mutext_mipi_send_up[i]);
                l_u8fifoin = g_mipi_to_up_queue[i].fifoin;
                pthread_mutex_unlock(&g_Mutext_mipi_send_up[i]);

                if (l_u8fifoin != g_mipi_to_up_queue[i].fifoout)
                {
                    l_u8IPC_data_flag = 1;                          // Got new mipi data from IPC

                    // 获取上位机针对上传mipi数据的指令信息
                    memcpy(&mipi_data_req, &g_mipi_req_info, sizeof(ORIGNAL_MIPI_REQ));

                    // 上传计数值初始化
                    l_u32_total_frame_num = mipi_data_req.req_mipi_num;
                    l_u32_frame_count = 0;
                    l_u16_total_package_num = l_u32_total_frame_num * (FRAME_LEN / SEND_DATA_LEN);
                    l_u16_package_count = 0;
                    l_u32temp_buf_len = 0;
                    l_u32snd_buf_len = 0;
                    l_u32buf_idx = 0;
                    p_frame_cnt = NULL;
                    p_package_cnt = NULL;
                    l_u16_crc = 0;

                    if (l_u8IPC_data_flag)
                    {
                        break;
                    }
                }

                if (l_u8IPC_data_flag)
                {
                    break;
                }
            }
        }

        // 检测到有新的半帧mipi数据保存
        if (l_u8IPC_data_flag)
        {   
            // 数据处理前，队列里面无效数据次数次数清0,该数据大于等于队列数，说明队列面已经没有有效数据了
            l_u32mipi_data_invald_cnt = 0;

            // 预留出保存一帧完整的mipi数据过程中，前后半帧被保存完成的时间差，理论上来看，预留的时间已经大于了这里usleep的时间了
            usleep(20);
            for (i = 0; i < MIPI_CHANNEL_MAX; i++)
            {
                    // 进一步检测是否有完整一帧的mipi数据被保存
                    if ((g_mipi_to_up_queue[i].m_u8valid[g_mipi_to_up_queue[i].fifoout] == VALID_VALUE) && 
                        (g_mipi_to_up_queue[i^1].m_u8valid[g_mipi_to_up_queue[i^1].fifoout] == VALID_VALUE))	//available
                    {
                                                // 上传的mipi数据达到指令要求的帧数后，将多保存的数据都丢掉
                        if (l_u32_frame_count >= l_u32_total_frame_num) 
                        {
                            g_mipi_to_up_queue[i].m_u8valid[g_mipi_to_up_queue[i].fifoout] == INVALID_VALUE;
                            g_mipi_to_up_queue[i^1].m_u8valid[g_mipi_to_up_queue[i^1].fifoout] == INVALID_VALUE;
                            g_mipi_to_up_queue[i].fifoout = (g_mipi_to_up_queue[i].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                            g_mipi_to_up_queue[i^1].fifoout = (g_mipi_to_up_queue[i^1].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                            printf("upload mipi data is enough, data give up:%d.\n",l_u32_total_frame_num);
                            continue;
                        }
                        // 检测前后两个半帧的mipi数据的长度是否符合要求，有半帧不符合要求，则丢掉整帧数据
                        l_u32Length_1 = g_mipi_to_up_queue[i].m_u32length[g_mipi_to_up_queue[i].fifoout];
                        l_u32Length_2 = g_mipi_to_up_queue[i^1].m_u32length[g_mipi_to_up_queue[i^1].fifoout];
                        if ((l_u32Length_1 != FRAME_LEN) || (l_u32Length_2 != FRAME_LEN))
                        {
                            g_mipi_to_up_queue[i].m_u8valid[g_mipi_to_up_queue[i].fifoout] == INVALID_VALUE;
                            g_mipi_to_up_queue[i^1].m_u8valid[g_mipi_to_up_queue[i^1].fifoout] == INVALID_VALUE;
                            g_mipi_to_up_queue[i].fifoout = (g_mipi_to_up_queue[i].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                            g_mipi_to_up_queue[i^1].fifoout = (g_mipi_to_up_queue[i^1].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                            printf("mipi data length is error, l_u32Length_1 = %u, l_u32Length_2 = %u\n", l_u32Length_1, l_u32Length_2);
                            continue;
                        }
                        // 暂未用到保存的socket
                        // l_s32fd = g_mipi_to_up_queue[i].m_i8fd[g_mipi_to_up_queue[i].fifoout];
                        // 填充网络帧头表信息
                        l_u8ret = fill_net_frame_head_table(g_net_recv_send_id, CIRCULAR_RESPONSE_INFO, ORIGINAL_MIPI_DATA, l_u8temp_buf, sizeof(l_u8temp_buf));
                        if (l_u8ret)
                        {
                            printf("fill_net_frame_head_table failed.\n");
                            continue;
                        }
                        l_u32buf_idx = sizeof(NET_FRAME_HEAD_TAB);
                        // 网络帧数据部分填充
                        memcpy(&l_u8temp_buf[l_u32buf_idx], &l_u32_total_frame_num, sizeof(l_u32_total_frame_num));
                        l_u32buf_idx += sizeof(l_u32_total_frame_num);
                        memcpy(&l_u8temp_buf[l_u32buf_idx], &l_u32_frame_count, sizeof(l_u32_frame_count));
                        l_u32buf_idx += sizeof(l_u32_frame_count);
                        memcpy(&l_u8temp_buf[l_u32buf_idx], g_mipi_to_up_queue[l_u32front_frame_mipi].m_utctime[g_mipi_to_up_queue[l_u32front_frame_mipi].fifoout], 2 * sizeof(uint32_t));
                        l_u32buf_idx += (2 * sizeof(uint32_t));
                        memcpy(&l_u8temp_buf[l_u32buf_idx], &l_u16_total_package_num, sizeof(l_u16_total_package_num));
                        l_u32buf_idx += sizeof(l_u16_total_package_num);
                        l_u32temp_buf_idx = l_u32buf_idx;   // 为了给下面的for循环多次复用
                        // 确保先发mipi_0后发mipi_1或者先发mipi_2后发mipi_3
                        if (i < (i^1))
                        {
                            l_u32front_frame_mipi = i;
                            l_u32after_frame_mipi = (i^1);
                        }
                        else
                        {
                            l_u32front_frame_mipi = (i^1);
                            l_u32after_frame_mipi = i;
                        }
                        // 先发前半帧
                        for (m = 0; m < (FRAME_LEN / SEND_DATA_LEN); m++)
                        {
                            l_u32buf_idx = l_u32temp_buf_idx;
                            l_u16_package_count = m;
                            memcpy(&l_u8temp_buf[l_u32buf_idx], &l_u16_package_count, sizeof(l_u16_package_count));
                            l_u32buf_idx += sizeof(l_u16_package_count);
                            memcpy(&l_u8temp_buf[l_u32buf_idx], &g_mipi_to_up_queue[l_u32front_frame_mipi].m_u8buffer[g_mipi_to_up_queue[l_u32front_frame_mipi].fifoout][m * SEND_DATA_LEN], SEND_DATA_LEN);
                            l_u32buf_idx += SEND_DATA_LEN;
                            l_u16_crc = bsp_crc16_xmodem_calculate(l_u8temp_buf, l_u32buf_idx);
                            l_u32buf_idx += sizeof(l_u16_crc);
                            l_u8ret = encode(l_u8temp_buf, l_u32buf_idx, l_u8snd_buf, sizeof(l_u8snd_buf), &l_u32snd_buf_len);
                            if (l_u8ret != 0)
                            {
                                printf("net send udp front half frame mipi data encode failed, err = %u.\n", l_u8ret);
                                continue;
                            }
                            /* 检测编码后的长度 */
                            if (l_u32snd_buf_len > 65507)   // UDP发送的数据包，在应用层，最大只能是65507Byte
                            {
                                printf("net send udp front half frame mipi data too long, data size is %u Byte.\n", l_u32snd_buf_len);
                                continue;
                            }
                            /* 网络发送数据 */
                            if (net_send_udp_data(l_u8snd_buf, l_u32snd_buf_len, p_sockaddr, SOCKET_CMD_TYPE) != 0)
                            {
                                printf("net send udp front half frame mipi data failed!\n");
                                continue;
                            }
                        }
                        // 再发后半帧
                        for (m = 0; m < (FRAME_LEN / SEND_DATA_LEN); m++)
                        {
                            l_u32buf_idx = l_u32temp_buf_idx;
                            l_u16_package_count =  (m + (FRAME_LEN / SEND_DATA_LEN));
                            memcpy(&l_u8temp_buf[l_u32buf_idx], &l_u16_package_count, sizeof(l_u16_package_count));
                            l_u32buf_idx += sizeof(l_u16_package_count);
                            memcpy(&l_u8temp_buf[l_u32buf_idx], &g_mipi_to_up_queue[l_u32after_frame_mipi].m_u8buffer[g_mipi_to_up_queue[l_u32after_frame_mipi].fifoout][m * SEND_DATA_LEN], SEND_DATA_LEN);
                            l_u32buf_idx += SEND_DATA_LEN;
                            l_u16_crc = bsp_crc16_xmodem_calculate(l_u8temp_buf, l_u32buf_idx);
                            l_u32buf_idx += sizeof(l_u16_crc);
                            l_u8ret = encode(l_u8temp_buf, l_u32buf_idx, l_u8snd_buf, sizeof(l_u8snd_buf), &l_u32snd_buf_len);
                            if (l_u8ret != 0)
                            {
                                printf("net send udp after half frame mipi data encode failed, err = %u.\n", l_u8ret);
                                continue;
                            }
                            /* 检测编码后的长度 */
                            if (l_u32snd_buf_len > 65507)   // UDP发送的数据包，在应用层，最大只能是65507Byte
                            {
                                printf("net send udp after half frame mipi data too long, data size is %u Byte.\n", l_u32snd_buf_len);
                                continue;
                            }
                            /* 网络发送数据 */
                            if (net_send_udp_data(l_u8snd_buf, l_u32snd_buf_len, p_sockaddr, SOCKET_CMD_TYPE) != 0)
                            {
                                printf("net send udp after half frame mipi data failed!\n");
                                continue;
                            }
                        }
                        
                        /* 等待上位机回复本帧mipi数据是否上传成功 */
                        clock_gettime(CLOCK_REALTIME,&ts);//real wall time
                        ts.tv_sec += timeout/1000;
                        ts.tv_nsec += (timeout%1000)*1000000;
                        sem_ret = sem_timedwait((sem_t *)&g_mipi_send_res_semaphore,&ts ); //interrupt
                        if(sem_ret != 0)//timeout or other
                        {
                            printf("wait mipi upload response failed.\n");
                            g_mipi_upload_status = 2;
                        }

                        /* 根据上位机回复的上传结果进行操作 */
                        if ((g_mipi_upload_status == 0) && (g_mipi_frame_index_rev == l_u32_frame_count))
                        {   
                            printf("up %u fifoo%d\n", l_u32_frame_count,g_mipi_to_up_queue[i].fifoout);             
                            g_mipi_to_up_queue[i].m_u8valid[g_mipi_to_up_queue[i].fifoout] == INVALID_VALUE;
                            g_mipi_to_up_queue[i^1].m_u8valid[g_mipi_to_up_queue[i^1].fifoout] == INVALID_VALUE;
                            g_mipi_to_up_queue[i].fifoout = (g_mipi_to_up_queue[i].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                            g_mipi_to_up_queue[i^1].fifoout = (g_mipi_to_up_queue[i^1].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                            l_u32_frame_count++;
                            l_u32reupload_cnt = 0;
                        }
                        else    // 重新上传
                        {
                            l_u32reupload_cnt++;
                            // 最多重发3次
                            if (l_u32reupload_cnt > 10)
                            {
                                // g_mipi_to_up_queue[i].m_u8valid[g_mipi_to_up_queue[i].fifoout] == INVALID_VALUE;
                                // g_mipi_to_up_queue[i^1].m_u8valid[g_mipi_to_up_queue[i^1].fifoout] == INVALID_VALUE;
                                // g_mipi_to_up_queue[i].fifoout = (g_mipi_to_up_queue[i].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                                // g_mipi_to_up_queue[i^1].fifoout = (g_mipi_to_up_queue[i^1].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                                // printf("%uth frame mipi reupload cnt max.\n", l_u32_frame_count);
                                // l_u32_frame_count++;
                                // l_u32reupload_cnt = 0;
                                l_u8IPC_data_flag = 0;
                                clear_fifo_flag_end_send(i); //清除0，1（远），还是2、3（近）
                            }
                            else
                            {
                                i--;
                                printf("%u:%dth frame mipi %u time reupload:%d.\n", l_u32_frame_count,g_mipi_frame_index_rev, l_u32reupload_cnt,g_mipi_upload_status);
                            }
                            continue;
                        }
                    }
                    else if ((g_mipi_to_up_queue[i].m_u8valid[g_mipi_to_up_queue[i].fifoout] == VALID_VALUE) || 
                        (g_mipi_to_up_queue[i^1].m_u8valid[g_mipi_to_up_queue[i^1].fifoout] == VALID_VALUE))
                    {
                        // 预防收到半帧mipi数据的情况，只收到半帧mipi数据时，直接将数据丢掉
                        // g_mipi_to_up_queue[i].m_u8valid[g_mipi_to_up_queue[i].fifoout] == INVALID_VALUE;
                        // g_mipi_to_up_queue[i^1].m_u8valid[g_mipi_to_up_queue[i^1].fifoout] == INVALID_VALUE;
                        // g_mipi_to_up_queue[i].fifoout = (g_mipi_to_up_queue[i].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                        // g_mipi_to_up_queue[i^1].fifoout = (g_mipi_to_up_queue[i^1].fifoout + 1)%QUEUE_RECV_DATA_NUM;
                        thread_uSleep(1000);
                        printf("Only recv and save half frame mipi data, give up the data.\n");
                    }
                    else    // 两个能组帧的队列对应位置都没有数据
                    {
                        l_u32mipi_data_invald_cnt++;
                        // printf("l_u32mipi_data_invald_cnt = %u.\n", l_u32mipi_data_invald_cnt);
                        if (l_u32mipi_data_invald_cnt >= (MIPI_CHANNEL_MAX * MIPI_RECV_THREAD_NUM))
                        {
                            l_u8IPC_data_flag = 0;
                            printf("queue have no data to process.\n");
                        }
                    }
            }
            if (l_u32_frame_count >= l_u32_total_frame_num) 
            {
                l_u8IPC_data_flag = 0;
            }
        }
    }

}


/*****************************************************************************/
/**
*
* 发送目标轨迹信息数据任务 net send target info data thread
*
* @param    None.
*
* @return   None
*
* @note     None.
*
******************************************************************************/
void *app_net_send_info_thread()
{
    char l_c_thread_name[26] = "app_net_send_target_info";
    IDENTITY recv_id_t = {0};
    message_node *p_msg_node = NULL;
    TRACK_INFO *p_target_info = NULL;
    uint32_t l_u32ret = 0;
    uint8_t l_u8tmp_buf[32768] = {0};
    uint8_t l_u8snd_buf[32768] = {0};
    uint32_t l_u32tmp_buf_len = 0, l_u32snd_buf_len = 0;
    uint16_t l_u16_crc = 0;
    struct sockaddr * p_sockaddr = NULL;
    
    prctl(PR_SET_NAME, l_c_thread_name);

    // 目标信息上报的接收方标识信息
    memset(recv_id_t.AreaCode, 0x00, sizeof(recv_id_t.AreaCode));
    recv_id_t.type = MEC;
    recv_id_t.num = 0x01;

    while (1)
    {
        // 从消息队列中取出数据
        p_msg_node = take_out_from_queue(&g_track_info_queue);
        p_target_info = (TRACK_INFO *)(p_msg_node->data);

        // 填充网络帧头表信息
        l_u32ret = fill_net_frame_head_table(recv_id_t, PROACT_UPLOAD, TRAFFIC_TARGET_TRAJ_INFO, l_u8tmp_buf, sizeof(l_u8tmp_buf));
        if (l_u32ret)
        {
            printf("app_net_send_info_thread fill_net_frame_head_table failed.\n");
            // 释放消息队列中申请的内存
            if (p_msg_node)
            {
                free(p_msg_node);
            }
            continue;
        }
        l_u32tmp_buf_len = sizeof(NET_FRAME_HEAD_TAB);

        // 填充UTC时间
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], p_target_info->UTC_time, sizeof(p_target_info->UTC_time));
        l_u32tmp_buf_len += sizeof(p_target_info->UTC_time);

        // 填充目标个数
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], &p_target_info->target_num, sizeof(p_target_info->target_num));
        l_u32tmp_buf_len += sizeof(p_target_info->target_num);

        // 填充目标信息
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], p_target_info->target, p_target_info->target_num * sizeof(p_target_info->target[0]));
        l_u32tmp_buf_len += (p_target_info->target_num * sizeof(p_target_info->target[0]));

        // 释放消息队列中申请的内存
        if (p_msg_node)
        {
            free(p_msg_node);
        }

        // 填充CRC校验位
        l_u16_crc = bsp_crc16_xmodem_calculate(l_u8tmp_buf, l_u32tmp_buf_len);
        l_u32tmp_buf_len += sizeof(l_u16_crc);

        // 数据编码
        l_u32ret = encode(l_u8tmp_buf, l_u32tmp_buf_len, l_u8snd_buf, sizeof(l_u8snd_buf), &l_u32snd_buf_len);
        if (l_u32ret != 0)
        {
            printf("net send target info data encode failed, err = %u.\n", l_u32ret);
            continue;
        }

        // 检测编码后的长度
        if (l_u32snd_buf_len > 65507)   // UDP发送的数据包，在应用层，最大只能是65507Byte
        {
            printf("net send target info data too long, data size is %u Byte.\n", l_u32snd_buf_len);
            continue;
        }

        // 网络发送数据
        if (net_send_udp_data(l_u8snd_buf, l_u32snd_buf_len, p_sockaddr, SOCKET_CMD_TYPE) != 0)
        {
            printf("net send target info data failed!\n");
            continue;
        }

    }

}


/*****************************************************************************/
/**
*
* 发送dsp原始目标信息数据任务 net send dsp info data thread
*
* @param    None.
*
* @return   None
*
* @note     None.
*
******************************************************************************/
void *app_net_send_dsp_thread()
{
    char l_c_thread_name[26] = "app_net_send_dsp_info";
    IDENTITY recv_id_t = {0};
    message_node *p_msg_node = NULL;
    UPLOAD_DSP_DATA *p_dsp_info = NULL;
    uint32_t l_u32ret = 0;
    uint8_t l_u8tmp_buf[32768] = {0};
    uint8_t l_u8snd_buf[32768] = {0};
    uint32_t l_u32tmp_buf_len = 0, l_u32snd_buf_len = 0;
    uint16_t l_u16_crc = 0;
    struct sockaddr * p_sockaddr = NULL;
    
    prctl(PR_SET_NAME, l_c_thread_name);

    // 目标信息上报的接收方标识信息
    memset(recv_id_t.AreaCode, 0x00, sizeof(recv_id_t.AreaCode));
    recv_id_t.type = MEC;
    recv_id_t.num = 0x01;

    while (1)
    {
        // 从消息队列中取出数据
        p_msg_node = take_out_from_queue(&g_dsp_info_queue);
        p_dsp_info = (UPLOAD_DSP_DATA *)(p_msg_node->data);

        // 填充网络帧头表信息
        l_u32ret = fill_net_frame_head_table(recv_id_t, CIRCULAR_RESPONSE_INFO, DSP_OUTPUT_DATA, l_u8tmp_buf, sizeof(l_u8tmp_buf));
        if (l_u32ret)
        {
            printf("app_net_send_dsp_thread fill_net_frame_head_table failed.\n");
            // 释放消息队列中申请的内存
            if (p_msg_node)
            {
                free(p_msg_node);
            }
            continue;
        }
        l_u32tmp_buf_len = sizeof(NET_FRAME_HEAD_TAB);
        
        // 要求上传的总帧数（暂时未用）
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], &p_dsp_info->total_frame_num, sizeof(p_dsp_info->total_frame_num));
        l_u32tmp_buf_len += sizeof(p_dsp_info->total_frame_num);

        // 上传的帧序号（暂时未用）
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], &p_dsp_info->frame_count, sizeof(p_dsp_info->frame_count));
        l_u32tmp_buf_len += sizeof(p_dsp_info->frame_count);

        // 填充UTC时间
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], p_dsp_info->UTC_time, sizeof(p_dsp_info->UTC_time));
        l_u32tmp_buf_len += sizeof(p_dsp_info->UTC_time);

        // 填充目标个数
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], &p_dsp_info->target_num, sizeof(p_dsp_info->target_num));
        l_u32tmp_buf_len += sizeof(p_dsp_info->target_num);

        // 填充目标信息
        memcpy(&l_u8tmp_buf[l_u32tmp_buf_len], p_dsp_info->dsp_single_target_info, p_dsp_info->target_num * sizeof(p_dsp_info->dsp_single_target_info[0]));
        l_u32tmp_buf_len += (p_dsp_info->target_num * sizeof(p_dsp_info->dsp_single_target_info[0]));

        // 释放消息队列中申请的内存
        if (p_msg_node)
        {
            free(p_msg_node);
        }

        // 填充CRC校验位
        l_u16_crc = bsp_crc16_xmodem_calculate(l_u8tmp_buf, l_u32tmp_buf_len);
        l_u32tmp_buf_len += sizeof(l_u16_crc);

        // 数据编码
        l_u32ret = encode(l_u8tmp_buf, l_u32tmp_buf_len, l_u8snd_buf, sizeof(l_u8snd_buf), &l_u32snd_buf_len);
        if (l_u32ret != 0)
        {
            printf("net send dsp info data encode failed, err = %u.\n", l_u32ret);
            continue;
        }

        // 检测编码后的长度
        if (l_u32snd_buf_len > 65507)   // UDP发送的数据包，在应用层，最大只能是65507Byte
        {
            printf("net send dsp info data too long, data size is %u Byte.\n", l_u32snd_buf_len);
            continue;
        }

        // 网络发送数据
        if (net_send_udp_data(l_u8snd_buf, l_u32snd_buf_len, p_sockaddr, SOCKET_CMD_TYPE) != 0)
        {
            printf("net send dsp info data failed!\n");
            continue;
        }

    }

}